﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using System.Drawing;
using System.Diagnostics;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningAlgorithms;

namespace MotionPlanning.MotionPlanningExperiments
{
    abstract class Experiment
    {
        protected int console_size;
        protected int console_first;
        protected int console_current;
        protected string[] console;
        public string ConsoleAssemble()
        {
            StringBuilder sbr = new StringBuilder(1024);
            int i = console_first;
            if (i < 0) i = 0;
            for (int j = 0; j < console_size; ++j)
            {
                string c = console[i];
                if (c == null) break;
                sbr.Append(c).Append(Environment.NewLine);

                i++;
                if (i >= console_size) i = 0;
            }
            return sbr.ToString();
        }
        public void Log(string msg)
        {
            lock (this)
            {
                if (msg == null) return;
                console[console_current] = msg;
                console_current++;
                console_first++;
                if (console_current >= console_size) console_current = 0;
                if (console_first >= console_size) console_first = 0;
            }
        }

        protected Stopwatch stopwatch;
        protected int iterations_requirement;
        protected string experiment_name;
        protected Thread workingThread;
        protected Thread monitoringThread;
        protected Thread visualThread;
        private bool keepRendering = true;
        protected Planner_RRTBased workingPlanner;
        abstract protected void experimentInstructions();
        virtual protected void experimentThreadFunction()
        {
            LogAndFlush("Experiment starts...");
            UIForm.BeginInvoke(new Action(UIOnStart));
            stopwatch.Start();
            experimentInstructions();
            stopwatch.Stop();
            LogAndFlush("Experiment finished.");
            keepRendering = false;
            UIForm.BeginInvoke(new Action(UIOnFinish));
        }

        protected Bitmap bitmap;
        public Graphics bitmap_canvas;
        public Graphics UIForm_canvas;

        protected Form1 UIForm;
        protected RichTextBox console_box;
        protected Button button_start;

        private int image_id;
        private bool goSave;

        protected StringBuilder monitor_msg;

        private void UIConsoleRefresh()
        {
            console_box.Text = ConsoleAssemble();
        }
        public void FlushConsole()
        {
            UIForm.BeginInvoke(new Action(UIConsoleRefresh));
        }
        public void LogAndFlush(string msg)
        {
            Log(msg);
            FlushConsole();
        }
        public void UICanvasRefresh_primary()
        {
            lock (bitmap)
            {
                UIForm_canvas.DrawImage(bitmap, 0, 0);
            }
        }
        public void FlushCanvas_primary()
        {
            UIForm.BeginInvoke(new Action(UICanvasRefresh_primary));
            //LogAndFlush("canvas flushed.");
        }

        public Experiment(string name, Planner_RRTBased planner, Form1 form, int iterations, int console_size = 25)
        {
            if (name == null || name.Length < 2)
                throw new Exception("experiment name must be filled.");
            experiment_name = name;
            if (console_size <= 0)
                throw new Exception("console_size must be larger than 1.");
            this.console_size = console_size;

            iterations_requirement = iterations;
            console_current = 0;
            console_first = console_current - console_size;
            console = new string[console_size];

            stopwatch = new Stopwatch();
            workingPlanner = planner;
            workingThread = new Thread(new ThreadStart(experimentThreadFunction));
            workingThread.Priority = ThreadPriority.Highest;
            visualThread = new Thread(new ThreadStart(visualRenderThreadFunction));
            visualThread.Priority = ThreadPriority.Lowest;
            monitoringThread = new Thread(new ThreadStart(monitoringThreadFunction));
            monitoringThread.Priority = ThreadPriority.Lowest;

            UIForm = form;
            this.button_start = UIForm.get_button_start();
            this.console_box = UIForm.get_console_box();

            this.button_start.Enabled = true;

            Visualizer vis = (Visualizer)workingPlanner.GetVisualizer();
            bitmap = new Bitmap(vis.GetCanvasSize().Width, vis.GetCanvasSize().Height);
            bitmap_canvas = Graphics.FromImage(bitmap);
            vis.SetCanvas(bitmap_canvas);
            UIForm_canvas = UIForm.get_canvas_primary();

            monitor_msg = new StringBuilder();
            n_render = 0;
            keepRendering = true;
            goSave = false;
            image_id = 0;
            LogAndFlush(experiment_name + " ready...");
        }
        
        virtual public void Start()
        {
            if (workingThread.ThreadState == System.Threading.ThreadState.Unstarted)
            {
                visualThread.Start();
                workingThread.Start();
                monitoringThread.Start();
            }
        }

        virtual public void Purge()
        {
            if (workingThread.ThreadState != System.Threading.ThreadState.Unstarted)
                workingThread.Abort();
            if (visualThread.ThreadState != System.Threading.ThreadState.Unstarted)
                visualThread.Abort();
            if (monitoringThread.ThreadState != System.Threading.ThreadState.Unstarted)
                monitoringThread.Abort();
        }

        private void UIOnStart()
        {
            this.button_start.Enabled = false;
        }
        private void UIOnFinish()
        {
            this.button_start.Enabled = true;
            this.button_start.Enabled = false;
        }

        virtual protected void monitoringThreadFunction()
        {
            while (true)
            {
                monitor_msg.Clear();
                int iteration = workingPlanner.Get_iterations_total();
                monitor_msg.Append("iteration = " + iteration)
                    .Append("\nvertices = " + workingPlanner.GetVertices().GetSize())
                    .Append("\nprogress = " + Math.Floor(iteration * 1000.0 / iterations_requirement) / 10 + "%")
                    .Append("\ntime(ms) = " + stopwatch.Elapsed.TotalMilliseconds)
                    .Append("\n")
                    .Append(workingPlanner.MonitorPlannerState());
                UIForm.BeginInvoke(new Action(monitorRefresh));

                Thread.Sleep(100);
            }
        }
        protected void monitorRefresh()
        {
            UIForm.get_monitor_box().Text = monitor_msg.ToString();
        }

        protected int n_render;

        protected void visualRenderThreadFunction()
        {
            LogAndFlush("Rendering starts...");
            int sleepMillSecondes = 100;
            while (keepRendering)
            {
                LogAndFlush("    rendering... " + n_render);
                n_render++;

                workingPlanner.GetVisualizer().RenderFrame();
                FlushCanvas_primary();

                if (goSave)
                {
                    StringBuilder sbr = new StringBuilder();
                    sbr.Append("../../MotionPlanningOutput/")
                        .Append(experiment_name)
                        .Append('_')
                        .Append(image_id.ToString("D4"))
                        .Append(".png");
                    SaveBitmap(sbr.ToString());
                    image_id++;
                }

                //PauseOrResume(false);
                
                Thread.Sleep(sleepMillSecondes);
            }
            LogAndFlush("Rendering finished.");
        }

        private void SaveBitmap(string name)
        {
            lock (bitmap)
            {
                bitmap.Save(name, System.Drawing.Imaging.ImageFormat.Png);
            }
        }

        public Planner_RRTBased get_CurrentPlanner()
        {
            return workingPlanner;
        }
    }
}
